
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "fibheap.h"

#ifdef MATLAB_MEX_FILE   
#include "mex.h"
#define debug_error(s) mexPrintf(s)
#else
#define debug_error(s) printf(s)
#endif

/*
 Make-Fibonacci-Heap()
    n[H] := 0
    min[H] := NIL 
 return H
 */
fibHeap* make_fibheap() {
    fibHeap *h;
    h = malloc(sizeof(fibHeap));
    h->n = 0;
    h->minNode = NULL;
    h->rootlist = dll_new_list();
    
    return h;
}

int free_fibheap(fibHeap *h) {
    dll_free_list(h->rootlist);
    free(h);
    return 0;
}

fibNode* make_fibnode(double key, unsigned int id) {
    fibNode *node;
    node=malloc(sizeof(fibNode));
    node->id=id;
    node->key=key;
    node->degree=0;
    node->mark=0;
    node->child=NULL;
    node->parent=NULL;
    
    node->lnode=dll_new_node((void *)node);
    
    return node;
}

void free_fibnode(fibNode *node) {
    dll_free_node(node->lnode);
    free(node);
}

/*
 Fibonacci-Heap-Minimum(H)
 return min[H]
 */
double fibheap_minimum(fibHeap *h) {
    return (h->minNode->key);
}

/*
 Fibonacci-Heap-Link(H,y,x)
    remove y from the root list of H
    make y a child of x
    degree[x] := degree[x] + 1
    mark[y] := FALSE
 */
void fibheap_link(fibHeap *h, fibNode *y, fibNode *x) {
    /*mexPrintf("LINK\n");*/
    
    dll_remove(h->rootlist, y->lnode);
    
    if (x->child == NULL)
        x->child = dll_new_list();
    
    dll_insertBeginning(x->child, y->lnode);
        
    y->parent = x;
    
    x->degree = x->degree+1;
    y->mark = 0;
}

/*
 CONSOLIDATE(H)
    for i:=0 to D(n[H])
         Do A[i] := NIL
    for each node w in the root list of H
        do x:= w
           d:= degree[x]
           while A[d] <> NIL
               do y:=A[d]
                  if key[x]>key[y]
                    then exchange x<->y
                  Fibonacci-Heap-Link(H, y, x)
                  A[d]:=NIL
                 d:=d+1
           A[d]:=x
    min[H]:=NIL
    for i:=0 to D(n[H])
        do if A[i]<> NIL
              then add A[i] to the root list of H
                   if min[H] = NIL or key[A[i]]<key[min[H]]
                      then min[H]:= A[i]
 */
void fibheap_consolidate(fibHeap *h) {
    int i, d, D;
    fibNode *x, *y, *tmp, **A;
    DoublyLinkedNode *w,*wx;
    
    /*D = (int) log2((float)(h->n));*/
    D = (int)(log((float)(h->n))/log(2))+1;
    A = malloc(sizeof(fibNode *)*D);
    
    for (i=0; i<D; i++) {
        A[i]=NULL;
    }
    /*mexPrintf("D: %d Elements: %d\n", D, h->rootlist->n);*/
    
    w = h->rootlist->firstNode;
    while (w != NULL) {
        wx=w->next;
        
        x=w->data;
        d=x->degree;
        
        while(A[d]!=NULL) {
            y = A[d];
            if (x->key > y->key) {
                tmp = x;
                x = y;
                y = tmp;
            }
            fibheap_link(h, y, x);
            /*mexPrintf("\n");*/
            /*mexPrintf("Link %f to %f\n",x->key,y->key);*/
            /*fibheap_debug(h);*/
            /*mexPrintf("\n");*/
            
            A[d] = NULL;
            d++;
        }
        
        A[d] = x;
        
        w=wx;
    }
    
    /*fibheap_debug(h);*/
    
    /*mexPrintf("*********** CONTINUE Consolidate!\n");*/
    h->minNode=NULL;
    
    for (i=0; i<D; i++) {
        if (A[i] != NULL) {
            /*dll_insertBeginning(h->rootlist, A[i]->lnode);*/
            if (h->minNode == NULL || A[i]->key < h->minNode->key)
                h->minNode = A[i];
            /*mexPrintf("Consolidate AK:%f HM:%f \n", A[i]->key, h->minNode->key);*/
        }
    }
    /*mexPrintf("END!\n");*/
    /*fibheap_debug(h);*/
}

/*
 Fibonacci-Heap-Insert(H,x)
    degree[x] := 0
    p[x] := NIL
    child[x] := NIL
    left[x] := x
    right[x] := x
    mark[x] := FALSE
    concatenate the root list containing x with root list H
    if min[H] = NIL or key[x]<key[min[H]] then
        min[H] := x
    n[H]:= n[H]+1
 */
void fibheap_insert(fibHeap *h, fibNode *x) {
    x->degree=0;
    x->mark=0;
    x->parent=NULL;
    x->child=NULL;
    dll_insertBeginning(h->rootlist, x->lnode);
    if (h->minNode == NULL || x->key < h->minNode->key)
        h->minNode = x;
    h->n++;
}

/*
 Fibonacci-Heap-Extract-Min(H)
    z:= min[H]
    if x <> NIL
            then for each child x of z
                 do add x to the root list of H
                    p[x]:= NIL
                 remove z from the root list of H
                 if z = right[z]
                    then min[H]:=NIL
                    else min[H]:=right[z]
                         CONSOLIDATE(H)
                 n[H] := n[H]-1
    return z
 */
fibNode *fibheap_extractmin(fibHeap *h) {
    fibNode *z,*xf, *next;
    DoublyLinkedList *xlist;
    DoublyLinkedNode *x, *y;
    
    z = h->minNode;
    mexPrintf("ExtractMin Executing...\n");
    
    if (z != NULL) {
        xlist=z->child;
        
        if (xlist != NULL) {
            
            for(x = xlist->firstNode; x != NULL;) {
                y = x->next;
                xf=x->data;
                xf->parent = NULL;
                dll_insertBeginning(h->rootlist, x);
                
                x = y;
            }
        }
        
        
        
        if (z->lnode->next != NULL)
            next = z->lnode->next->data;
        else
            next = NULL;
        
        dll_remove(h->rootlist, z->lnode);
        h->n--;
        
        mexPrintf("SIZE: %d\n",h->n);
        
        if (h->n == 0)
            h->minNode = NULL;
        else {
            h->minNode = next;
            fibheap_consolidate(h);
        }
        
    }
    
    return z;
}

/*
 Fibonacci-Heap-Decrease-Key(H,x,k)
    if k > key[x]
       then error "new key is greater than current key"
    key[x] := k
    y := p[x]
    if y <> NIL and key[x]<key[y]
       then CUT(H, x, y)
            CASCADING-CUT(H,y)    
    if key[x]<key[min[H]]
       then min[H] := x
 */
void fibheap_decreasekey(fibHeap *h, fibNode *x, double k) {
    fibNode *y;
    
    if (k > x->key) {
        debug_error("Error: New key is greater than current key.\n");
        return;
    }
    
    x->key = k;
    y = x->parent;
    
    if (y != NULL && x->key < y->key) {
        fibheap_cut(h,x,y);
        fibheap_cascadingcut(h,y);
    }
    
    if (h->minNode != NULL && (x->key < h->minNode->key))
        h->minNode = x;
    
}

/*
 CUT(H,x,y)
    Remove x from the child list of y, decrementing degree[y]
    Add x to the root list of H
    p[x]:= NIL
    mark[x]:= FALSE
 */

void fibheap_cut(fibHeap *h, fibNode *x, fibNode *y) {
    
    dll_remove(y->child, x->lnode);
    y->degree--;
    dll_insertBeginning(h->rootlist, x->lnode);
    x->parent = NULL;
    x->mark=0;
}

/*
 CASCADING-CUT(H,y)
    z:= p[y]
    if z <> NIL
      then if mark[y] = FALSE
           then mark[y]:= TRUE
           else CUT(H, y, z)
                CASCADING-CUT(H, z)
 */
void fibheap_cascadingcut(fibHeap *h, fibNode *y) {
    fibNode *z;
    
    z = y->parent;
    if (z != NULL)
        if (y->mark == 0)
            y->mark = 1;
        else {
            fibheap_cut(h,y,z);
            fibheap_cascadingcut(h,z);
        }
}

void fibheap_debug(fibHeap *h) {
    DoublyLinkedNode *x, *x1, *y, *y1, *listf;
    fibNode *xnode, *xnode1;
    
    mexPrintf("DEBUG ----------------- \n");
    mexPrintf("ROOTLIST:\n");
    dll_debug(h->rootlist);
    
    mexPrintf("ALL childs\n");
    listf = h->rootlist->firstNode;
    
    for (x=listf; x != NULL;) {
        y = x->next;
        
        xnode = x->data;
        
        if (xnode->child != NULL) {
            mexPrintf("CHILD %f\n", xnode->key);
            dll_debug(xnode->child);
            
            for (x1=xnode->child->firstNode; x1 != NULL;) {
                y1 = x1->next;

                xnode1 = x1->data;

                if (xnode1->child != NULL) {
                    mexPrintf("CHILD_ %f\n", xnode1->key);
                    dll_debug(xnode1->child);
                }

                x1=y1;
            }
            mexPrintf("-----------\n");
        }
        
        x=y;
    }
        
    
    mexPrintf("END DEBUG ------------- \n");
    
}

